#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>

#define pi 3.1415926

using namespace cv;
using namespace std;
using namespace Eigen;

int main()
{
    //OpenCV
    Mat Roty45d = (Mat_<float>(4, 4) << cos(pi/4), 0, sin(pi/4), 0, 
                                        0, 1, 0, 0,
                                        -sin(pi/4), 0, cos(pi/4), 0,
                                        0, 0, 0, 1);
    Mat Rotz30d = (Mat_<float>(4, 4) << cos(pi/6), -sin(pi/6), 0, 0,
                                        sin(pi/6), cos(pi/6), 0, 0,
                                        0, 0, 1, 0,
                                        0, 0, 0, 1);
    Mat Trans = (Mat_<float>(4, 4) << 1, 0, 0, 2,
                                      0, 1, 0, -3,
                                      0, 0, 1, 1,
                                      0, 0, 0, 1);
    Mat Rotx60d = (Mat_<float>(4, 4) << 1, 0, 0, 0,
                                        0, cos(pi/3), -sin(pi/3), 0,
                                        0, sin(pi/3), cos(pi/3), 0,
                                        0, 0, 0, 1);
    Mat PB = (Mat_<float>(4, 1) << 1, 2, 0, 1);

    Mat RBA = Roty45d * Rotz30d * Trans * Rotx60d;
    Mat PA = RBA * PB;
    cout << "The homogeneous coordinate in A is(by OpenCV): " << endl << PA << endl;

    //Eigen
    AngleAxisd rotation_vector1(pi/6, Vector3d(0, 0, 1));
    Isometry3d T1 = Isometry3d::Identity();
    T1.rotate(rotation_vector1);

    Isometry3d T2 = Isometry3d::Identity();
    T2.pretranslate(Vector3d(2, -3, 1));

    AngleAxisd rotation_vector2(pi/4, Vector3d(0, 1, 0));
    Isometry3d T3 = Isometry3d::Identity();
    T3.rotate(rotation_vector2);

    AngleAxisd rotation_vector3(pi/3, Vector3d(1, 0, 0));
    Isometry3d T4 = Isometry3d::Identity();
    T4.rotate(rotation_vector3);

    Vector3d VB(1, 2, 0);
    Isometry3d TBA = T3 * T1 * T2 * T4;
    Vector3d VA = TBA * VB;

    cout << "The homogeneous coordinate in A is(by Eigen): " << endl << VA << endl;

    
    
    system("pause");
    return 0;
}
